/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package de.biomedical_imaging.ij.nlMeansPlugin;

import ij.IJ;
import ij.ImagePlus;
import ij.gui.GenericDialog;
import ij.plugin.filter.Convolver;
import ij.plugin.filter.PlugInFilter;
import static ij.plugin.filter.PlugInFilter.DOES_ALL;
import ij.process.ByteProcessor;
import ij.process.ColorProcessor;
import ij.process.FloatProcessor;
import ij.process.ImageProcessor;
import ij.process.ShortProcessor;
import java.awt.image.BufferedImage;
import java.awt.image.WritableRaster;
import java.util.ArrayList;
import java.util.List;

/**
 *
 * @author Pascal
 */
public class NLMeansDenoising_ implements PlugInFilter {
    
//    private final String newImageTitle = "NLMeans Result";
    private final int weightPrecision = 1000;
    
    private ImagePlus imp;
//    private ImagePlus impNew;
//    private ImageProcessor ip;
    private int width;
    private int height;
    
    private int[][] pixels;
    private int[][] pixelsExpand;
    private int widthE;
    private int heightE;
    
    private int w; // Big Window
    private int n; // Small Window for Patches
    
    private double sigma2;
    private double h2;
    private double distConst;
    private int dim;
    
    private int nextdx;
    private int nextdy;
    
    private int[][] uI;
    private double[][] uD;
    private int[] wMaxArrI;
    private int[] wSumArrI;
    private double[] wMaxArrD;
    private double[] wSumArrD;
    private boolean autoEstimate = false;
    private int constantSigma = 15;
    
//    private long startTime;
    
    @Override
    public int setup(String arg, ImagePlus imp) {
        this.imp = imp;
//        this.ip = imp.getProcessor();
        
        GenericDialog gd = new GenericDialog("Non-Local Means");
        gd.addNumericField("Sigma", 15, 0);
        
        gd.addCheckbox("Auto estimate sigma", false);
        gd.showDialog();
        if (gd.wasCanceled()) {
            return -1;
        }
        constantSigma = (int)gd.getNextNumber();
        autoEstimate = gd.getNextBoolean();

        return IJ.setupDialog(imp, DOES_ALL);
    }
    
    @Override
    public void run(ImageProcessor ip) {
        int sigma = constantSigma;
        if (autoEstimate) {
            sigma = (int)getGlobalNoiseLevel(new ImagePlus(null, ip));
        }

        initSettings(sigma, ip);

        try {
//            long normalS = System.nanoTime();
            double[][] result = NLMeansMultithreadDouble(pixels, Runtime.getRuntime().availableProcessors(), ip);
            createPicture(result, ip);
//            IJ.showMessage("Computation Time Normal Algorithm: " + ((System.nanoTime() - normalS) / 1000000000.0));
//            long normalE = System.nanoTime();
//            initSettings(sigma, ip);
//            long saveS = System.nanoTime();
//            NLMeansMultithreadNoOverflow(pixels, Runtime.getRuntime().availableProcessors(), ip);
//            IJ.showMessage("Computation Time Save Algorithm: " + ((System.nanoTime() - saveS) / 1000000000.0)
//             + "Computation Time Normal Algorithm: " + ((normalE - normalS) / 1000000000.0));
        } catch (InterruptedException e) {
            IJ.showMessage("Error while computing Denoised Image.");
        }
    }
    
    // #########################################################
    // ********** Integer Implementation of Algorithm **********
    // #########################################################
    
    /**
     * Multi Threaded Implementation of the Non-local Means Algorithm.
     * This accelerated Version is based off of: Darbon, Jérôme, et al. 
     * "Fast nonlocal filtering applied to electron cryomicroscopy." 
     * Biomedical Imaging: From Nano to Macro, 2008. ISBI 2008. 
     * 5th IEEE International Symposium on. IEEE, 2008.
     * @param image The image as Integer Array. Colors are stored within first 
     * dimension of Array. Gets computed via convertImage()
     * @param threadcount Number of Threads used for Denoising
     * @param ip ImageProcessor for the original Image
     * @throws InterruptedException 
     */
    private double[][] NLMeansMultithreadInt(int[][] image, int threadcount, ImageProcessor ip) throws InterruptedException  {
        pixelsExpand = expandImage(image, false);
        List<WorkerInt> workerList = new ArrayList<WorkerInt>(threadcount);
        for (int i = 0; i < threadcount; i++) {
            WorkerInt worker = new WorkerInt();
            worker.start();
            workerList.add(worker);
        }
        for (WorkerInt worker : workerList) {
            worker.join();
        }
        
        return finishPictureInt(uI, wMaxArrI, wSumArrI);
//        IJ.showMessage("Computation Time: " + ((System.nanoTime() - startTime) / 1000000000.0));
    }

    /**
     * This Method is used to deliver a partial Denoising result.
     * These results get computed by Threads. The results still need to be
     * divided by the sum of the weights, which happens in finishPicture(...)
     * @param imagePart 
     */
    private synchronized void deliverImagePartInt(int[][] imagePart) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                for (int d = 0; d < dim; d++) {
                    uI[d][offset + x] += imagePart[d][offset + x];
                }
            }  
        }
    }

    /**
     * This Method is used to deliver a partial result of the Weight Sum Array.
     * The Weight Sum Array stores the sum of all Weights that are used
     * for each pixel. It is used within finishPicture(...) to properly Scale
     * each Pixel.
     * @param arr Weight Sum Array
     */
    private synchronized void deliverWSumArrInt(int[] arr) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                wSumArrI[offset + x] += arr[offset + x];
            }  
        }
    } 
    
    /**
     * This Method is used to deliver a partial result of the Weight Max Array.
     * The Weight Max Array stores the maximum Weight that is used per Pixel.
     * This Weight is used as Weight between the Pixel and itself.
     * @param arr Maximum Weight Array
     */
    private synchronized void deliverWMaxArrInt(int[] arr) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                if (wMaxArrI[offset + x] < arr[offset + x]) {
                    wMaxArrI[offset + x] = arr[offset + x];
                }
            }  
        }
    }
    
    /**
     * Finishes the Picture by dividing every Pixel with the Sum of all Weights
     * for the respective Pixel, and by performing the last denoising step.
     * As last Step, the Pixels get weighted with the maximum Weight for each Pixel.
     * @param picture The Denoised Picture
     * @param wMaxArr Array with highest used Weight for each Pixel
     * @param wSumArr Array with Sum of Weights for each Pixel
     * @return 
     */
    private double[][] finishPictureInt(int[][] picture, int[] wMaxArr, int[] wSumArr) {
        double[][] result = new double[dim][width*height];
        int wn = w + n;
        
        // x and y coordinates are based off the original Image (NOT the expanded Image)
        for (int y = 0; y < height; y++) {
            int offset = y*width; // y offset for original Image coordinates
            int offset2 = (y + w + n)*widthE; // y offset for expanded Image coordinates
            for (int x = 0; x < width; x++) {
                int k = offset + x; // array Position for Pixel x, y
                int kwn = offset2 + x + wn; // same as k, but in expanded Image coordinates
                for (int d = 0; d < result.length; d++) {
                    result[d][k] = picture[d][kwn];
                    
                    if (wMaxArr[kwn] == 0) {
                        // If Sum of all Weights is 0, just copy the original Pixel
                        result[d][k] += pixelsExpand[d][kwn];
                    } else {
                        // Weight the original Pixel with the maximum Weight
                        result[d][k] += pixelsExpand[d][kwn] * wMaxArr[kwn];
                        wSumArr[kwn] += wMaxArr[kwn];
                        
                        // Divide Pixel by sum of all Weights
                        result[d][k] /= wSumArr[kwn];
                    }
                }
            }
        }
        
        return result;
    }
    
    /**
     * Denoising Step for a specific dx / dy Pair
     * Uses Difference Image for the given dx / dy Pair to perform a partial 
     * Denoising of the Image.
     * Takes Advantage of the symmetry of the Weights.
     * @param targetArr Array in which the weighted Pixels get written into
     * @param S Difference Image for the dx / dy Pair
     * @param wMaxArr Stores the Maximum Weight for each Pixel
     * @param wSumArr All Weights for each Pixel get Added into this Array
     * @param dx
     * @param dy 
     */
    private void denoiseInt(int[][] targetArr, int[][] S, int[] wMaxArr, int[] wSumArr, int dx, int dy) {
        int wn = w + n;
        
        // x and y coordinates are based off the expanded Image
        for (int y = wn; y < heightE - wn; y++) {
            int offset = y*widthE; // offset for y
            int offsetn = (y + dy)*widthE; // offset for y + dy
            for (int x = wn; x < widthE - wn; x++) {
                int k = offset + x; // array position for pixel x,y
                int kn = offsetn + x + dx; // array position for pixel x+dx, y+dy
                int weight = computeWeightInt(S, x, y, weightPrecision);
                wMaxArr[k] = Math.max(weight, wMaxArr[k]); // Max Weight for Pixel k
                wSumArr[k] += weight; // Weight Sum for Pixel k
                wMaxArr[kn] = Math.max(weight, wMaxArr[kn]); // Max Weight for Pixel kn
                wSumArr[kn] += weight; // Weight Sum for Pixel kn
                
                for (int d = 0; d < dim; d++) {
                    targetArr[d][k] += weight * pixelsExpand[d][kn];
                    targetArr[d][kn] += weight * pixelsExpand[d][k];
                }
            }
        }
    }
    
    /**
     * Computes the Weight between the Pixel x,y and the Pixel that lies
     * at x + dx, y + dy. dx and dy are implicitly given because the
     * Difference Image is based on them.
     * @param S Difference Image for a dx / dy pair
     * @param x x-Coordinate of the current Pixel
     * @param y y-Coordinate of the current Pixel
     * @param precision Precision of the Weight. Should be multiple of 10
     * @return 
     */
    private int computeWeightInt(int[][] S, int x, int y, int precision) {
        double distance = computeDistanceInt(S, x, y);

        double exp = FastMathStuff.max(distance-sigma2, 0.0);
        
//        exp /= h2;
//        double weight = Math.exp(-exp);
        double weight = h2 / (h2 + exp);
        
//        int iWeight = FastMathStuff.fastRound(weight * precision) + 1;
//        if (iWeight == 0) iWeight = 1;

        return FastMathStuff.fastRound(weight * precision);
    }
    
    /**
     * Computes the Difference between the Surroundings of the Pixel x,y and the 
     * Pixel that lies at x + dx, y + dy. dx and dy are implicitly given 
     * because the Difference Image is based on them.
     * Is used to compute the Weights. 
     * @param S Difference Image for a dx / dy pair
     * @param x x-Coordinate of the current Pixel
     * @param y y-Coordinate of the current Pixel
     * @return 
     */
    private double computeDistanceInt(int[][] S, int x, int y) {
        double distance = 0;
        for (int d = 0; d < dim; d++) {
            distance += S[d][(y + n) * widthE + (x + n)] 
                      + S[d][(y - n) * widthE + (x - n)]
                      - S[d][(y - n) * widthE + (x + n)]
                      - S[d][(y + n) * widthE + (x - n)];
        }

        return distance / distConst;
    }
    
    /**
     * Computes the Difference Image for a given dx / dy Pair. As dx and dy can
     * be negative, image needs to be expanded to prevent out of bounds errors.
     * @param image Expanded Version of Original Image
     * @param targetArr Target Array in which the Difference Image gets stored into
     * @param dx
     * @param dy 
     */
    private void computeDifferenceImageInt(int[][] image, int[][] targetArr, int dx, int dy) {
        int wn = w + n;
        int temp;
        int overflowCount = 0;
        
        // Compute very first Pixel of Image (x = 0; y = 0)
        for (int d = 0; d < dim; d++) {
            temp = image[d][wn * widthE + wn]
                        - image[d][(wn + dy)*widthE + dx + wn];
            targetArr[d][wn * widthE + wn] = temp * temp;
        }
        
        // Compute first Row of Image (y = 0)
        int offset = wn * widthE;
        int offsetdy = (wn + dy) * widthE;
        for (int x = wn + 1; x < widthE; x++) {
            for (int d = 0; d < dim; d++) {
                temp = image[d][offset + x] - image[d][offsetdy + x + dx];
                targetArr[d][offset + x] = targetArr[d][offset + x - 1] 
                        + temp * temp;
            }
        }
        
        // Compute first Column of Image (x = 0)
        for (int y = wn + 1; y < heightE; y++) {
            int offsety = y*widthE;
            offsetdy = (y + dy)*widthE;
            for (int d = 0; d < dim; d++) {
                temp = image[d][offsety + wn] - image[d][offsetdy + wn + dx];
                targetArr[d][offsety + wn] = targetArr[d][offsety - widthE + wn] + temp * temp;
            }
        }
        
        // Compute rest of the Image
        for (int y = wn + 1; y < heightE; y++) {
            offset = y*widthE;
            int offset2 = (y + dy)*widthE;
            for (int x = wn + 1; x < widthE; x++) {
                for (int d = 0; d < dim; d++) {
                    targetArr[d][offset + x]  = targetArr[d][offset + x - 1];
                    targetArr[d][offset + x] += targetArr[d][offset + x - widthE];
                    targetArr[d][offset + x] -= targetArr[d][offset + x - 1 - widthE];
                    
                    temp = image[d][offset + x] - image[d][offset2 + x + dx];
                    int temp2 = temp * temp;
                    if (Integer.MAX_VALUE - temp2 < targetArr[d][offset + x]) overflowCount++;
                    targetArr[d][offset + x] += temp2;
                }
            }
        }
        
//        if (overflowCount > 0) IJ.showMessage(overflowCount + "");
    }
    
    
    // ########################################################
    // ********** Double Implementation of Algorithm **********
    // ########################################################
    
    /**
     * Multi Threaded Implementation of the Non-local Means Algorithm.
     * This accelerated Version is based off of: Darbon, Jérôme, et al. 
     * "Fast nonlocal filtering applied to electron cryomicroscopy." 
     * Biomedical Imaging: From Nano to Macro, 2008. ISBI 2008. 
     * 5th IEEE International Symposium on. IEEE, 2008.
     * @param image The image as Integer Array. Colors are stored within first 
     * dimension of Array. Gets computed via convertImage()
     * @param threadcount Number of Threads used for Denoising
     * @param ip ImageProcessor for the original Image
     * @throws InterruptedException 
     */
    private double[][] NLMeansMultithreadDouble(int[][] image, int threadcount, ImageProcessor ip) throws InterruptedException  {
        pixelsExpand = expandImage(image, false);
        List<WorkerDouble> workerList = new ArrayList<WorkerDouble>(threadcount);
        for (int i = 0; i < threadcount; i++) {
            WorkerDouble worker = new WorkerDouble();
            worker.start();
            workerList.add(worker);
        }
        for (WorkerDouble worker : workerList) {
            worker.join();
        }
        
        return finishPictureDouble(uD, wMaxArrD, wSumArrD);

//        IJ.showMessage("Computation Time: " + ((System.nanoTime() - startTime) / 1000000000.0));
    }
    
    private synchronized void deliverImagePartDouble(double[][] imagePart) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                for (int d = 0; d < dim; d++) {
                    uD[d][offset + x] += imagePart[d][offset + x];
                }
            }  
        }
    }
    
    /**
     * This Method is used to deliver a partial result of the Weight Sum Array.
     * The Weight Sum Array stores the sum of all Weights that are used
     * for each pixel. It is used within finishPicture(...) to properly Scale
     * each Pixel.
     * @param arr Weight Sum Array
     */
    private synchronized void deliverWSumArrDouble(double[] arr) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                wSumArrD[offset + x] += arr[offset + x];
            }  
        }
    } 
    
    /**
     * This Method is used to deliver a partial result of the Weight Max Array.
     * The Weight Max Array stores the maximum Weight that is used per Pixel.
     * This Weight is used as Weight between the Pixel and itself.
     * @param arr Maximum Weight Array
     */
    private synchronized void deliverWMaxArrDouble(double[] arr) {
        for (int y = 0; y < heightE; y++) {
            int offset = y*widthE;
            for (int x = 0; x < widthE; x++) {
                if (wMaxArrD[offset + x] < arr[offset + x]) {
                    wMaxArrD[offset + x] = arr[offset + x];
                }
            }  
        }
    }
    
    /**
     * Finishes the Picture by dividing every Pixel with the Sum of all Weights
     * for the respective Pixel, and by performing the last denoising step.
     * As last Step, the Pixels get weighted with the maximum Weight for each Pixel.
     * @param picture The Denoised Picture
     * @param wMaxArr Array with highest used Weight for each Pixel
     * @param wSumArr Array with Sum of Weights for each Pixel
     * @return 
     */
    private double[][] finishPictureDouble(double[][] picture, double[] wMaxArr, double[] wSumArr) {
        double[][] result = new double[dim][width*height];
        int wn = w + n;
        
        // x and y coordinates are based off the original Image (NOT the expanded Image)
        for (int y = 0; y < height; y++) {
            int offset = y*width; // y offset for original Image coordinates
            int offset2 = (y + w + n)*widthE; // y offset for expanded Image coordinates
            for (int x = 0; x < width; x++) {
                int k = offset + x; // array Position for Pixel x, y
                int kwn = offset2 + x + wn; // same as k, but in expanded Image coordinates
                for (int d = 0; d < result.length; d++) {
                    result[d][k]  = picture[d][kwn];
                    
                    if (wMaxArr[kwn] == 0) {
                        // If Sum of all Weights is 0, just copy the original Pixel
                        result[d][k] += pixelsExpand[d][kwn];
                    } else {
                        // Weight the original Pixel with the maximum Weight
                        result[d][k] += pixelsExpand[d][kwn] * wMaxArr[kwn];
                        wSumArr[kwn] += wMaxArr[kwn];
                        
                        // Divide Pixel by sum of all Weights
                        result[d][k] /= wSumArr[kwn];
                    }
                }
            }
        }
        
        return result;
    }
    
    private void denoiseDouble(double[][] targetArr, double[][] S, double[] wMaxArr, 
            double[] wSumArr, int dx, int dy) {
        int wn = w + n;
        for (int y = wn; y < heightE - wn; y++) {
            int offset = y*widthE;
            int offsetn = (y + dy)*widthE; //
            for (int x = wn; x < widthE - wn; x++) {
                int k = offset + x;
                int kn = offsetn + x + dx;
                int weight = computeWeightDouble(S, x, y, weightPrecision);
                wMaxArr[k] = Math.max(weight, wMaxArr[k]);
                wSumArr[k] += weight;
                wMaxArr[kn] = Math.max(weight, wMaxArr[kn]);
                wSumArr[kn] += weight;
                
                for (int d = 0; d < dim; d++) {
                    int wk = weight * pixelsExpand[d][k];
                    int wkn = weight * pixelsExpand[d][kn];
                    if (Double.MAX_VALUE - wkn < targetArr[d][k]) {
                        // Overflow!
                        IJ.showMessage("Overflow!");
                    } else {
                        targetArr[d][k] += wkn;
                    }
                    if (Double.MAX_VALUE - wk < targetArr[d][kn]) {
                        // Overflow!
                        IJ.showMessage("Overflow!");
                    } else {
                        targetArr[d][kn] += wk;
                    }
                    
                }
            }
        }
    }
    
    /**
     * Computes the Weight between the Pixel x,y and the Pixel that lies
     * at x + dx, y + dy. dx and dy are implicitly given because the
     * Difference Image is based on them.
     * @param S Difference Image for a dx / dy pair
     * @param x x-Coordinate of the current Pixel
     * @param y y-Coordinate of the current Pixel
     * @param precision Precision of the Weight. Should be multiple of 10
     * @return 
     */
    private int computeWeightDouble(double[][] S, int x, int y, int precision) {
        double distance = computeDistanceDouble(S, x, y);

        double exp = FastMathStuff.max(distance-sigma2, 0.0);
        
//        exp /= h2;
//        double weight = Math.exp(-exp);
        double weight = h2 / (h2 + exp);
        
//        int iWeight = FastMathStuff.fastRound(weight * precision) + 1;
//        if (iWeight == 0) iWeight = 1;

        return FastMathStuff.fastRound(weight * precision);
    }
    
    /**
     * Computes the Difference between the Surroundings of the Pixel x,y and the 
     * Pixel that lies at x + dx, y + dy. dx and dy are implicitly given 
     * because the Difference Image is based on them.
     * Is used to compute the Weights. 
     * @param S Difference Image for a dx / dy pair
     * @param x x-Coordinate of the current Pixel
     * @param y y-Coordinate of the current Pixel
     * @return 
     */
    private double computeDistanceDouble(double[][] S, int x, int y) {
        double distance = 0;
        for (int d = 0; d < dim; d++) {
            distance += S[d][(y + n) * widthE + (x + n)] 
                      + S[d][(y - n) * widthE + (x - n)]
                      - S[d][(y - n) * widthE + (x + n)]
                      - S[d][(y + n) * widthE + (x - n)];
        }

        return distance / distConst;
    }
    
    /**
     * Computes the Difference Image for a given dx / dy Pair. As dx and dy can
     * be negative, image needs to be expanded to prevent out of bounds errors.
     * @param image Expanded Version of Original Image
     * @param targetArr Target Array in which the Difference Image gets stored into
     * @param dx
     * @param dy 
     */
    private void computeDifferenceImageDouble(int[][] image, double[][] targetArr, int dx, int dy) {
        int wn = w + n;
        double temp;
        int overflowCount = 0;
        
        // Compute very first Pixel of Image (x = 0; y = 0)
        for (int d = 0; d < dim; d++) {
            temp = image[d][wn * widthE + wn]
                        - image[d][(wn + dy)*widthE + dx + wn];
            targetArr[d][wn * widthE + wn] = temp * temp;
        }
        
        // Compute first Row of Image (y = 0)
        int offset = wn * widthE;
        int offsetdy = (wn + dy) * widthE;
        for (int x = wn + 1; x < widthE; x++) {
            for (int d = 0; d < dim; d++) {
                temp = image[d][offset + x] - image[d][offsetdy + x + dx];
                targetArr[d][offset + x] = targetArr[d][offset + x - 1] 
                        + temp * temp;
            }
        }
        
        // Compute first Column of Image (x = 0)
        for (int y = wn + 1; y < heightE; y++) {
            int offsety = y*widthE;
            offsetdy = (y + dy)*widthE;
            for (int d = 0; d < dim; d++) {
                temp = image[d][offsety + wn] - image[d][offsetdy + wn + dx];
                targetArr[d][offsety + wn] = targetArr[d][offsety - widthE + wn] + temp * temp;
            }
        }
        
        // Compute rest of the Image
        for (int y = wn + 1; y < heightE; y++) {
            offset = y*widthE;
            int offset2 = (y + dy)*widthE;
            for (int x = wn + 1; x < widthE; x++) {
                for (int d = 0; d < dim; d++) {
                    targetArr[d][offset + x]  = targetArr[d][offset + x - 1];
                    targetArr[d][offset + x] += targetArr[d][offset + x - widthE];
                    targetArr[d][offset + x] -= targetArr[d][offset + x - 1 - widthE];
                    
                    temp = image[d][offset + x] - image[d][offset2 + x + dx];
                    double temp2 = temp * temp;
                    if (Double.MAX_VALUE - temp2 < targetArr[d][offset + x]) overflowCount++;
                    targetArr[d][offset + x] += temp2;
                }
            }
        }
        
        if (overflowCount > 0) IJ.showMessage(overflowCount + "");
    }
    
    
    
    /**
     * Expands the boundaries of an image in all four directions. The new content
     * of the Image gets filled with the adjacent parts of the Image.
     * To view a Preview of this Image, use display = true
     * @param image Original Image
     * @param display Display Preview of generated Image
     * @return 
     */
    private int[][] expandImage(int[][] image, boolean display) {
        int[][] result = new int[dim][widthE * heightE];

        for (int y = 0; y < heightE; y++) {
            int yr = y - w - n;
            if (yr < 0) yr = height + yr;
            if (yr >= height) yr = yr - height;
            int offset = y * widthE;
            int offsetr = yr * width;
            for (int x = 0; x < widthE; x++) {
                int xr = x - w - n;
                if (xr < 0) xr = width + xr;
                if (xr >= width) xr = xr - width;
                for (int d = 0; d < dim; d++) {
                    result[d][offset + x] = image[d][offsetr + xr];
                }
            }
        }
        
        if (display) {
            int[] pixelsPicture = new int[result[0].length];
        
            for (int y = 0; y < heightE; y++) {
                int offset = y*widthE;
                for (int x = 0; x < widthE; x++) {
                    int p = offset + x;
                    int red = (int)result[0][p];
                    int green = (int)result[1][p];
                    int blue = (int)result[2][p];
                    int pixel = ((red & 0xff)<<16) 
                              + ((green & 0xff)<<8) 
                              + (blue & 0xff);
                    pixelsPicture[p] = pixel;
                }
            }
            
            BufferedImage bimg = convertToImage(widthE, heightE, pixelsPicture);
            ImagePlus imp2 = new ImagePlus("Expanded Image", bimg);
            imp2.show();
        }
        
        return result;
    }
    
    /**
      * Implements the gaussian noise level estimation algorithm of 
      * Immerkaer, J., 1996. Fast noise variance estimation. 
      * Computer Vision and Image Understanding, 
      * 64(2), pp.300–302. 
      * @param imp
      * @return noise level
      */
    public static double getGlobalNoiseLevel(ImagePlus imp){
        FloatProcessor fp = null;
        switch(imp.getType()) {
            
            case ImagePlus.COLOR_256:
                ByteProcessor bp = (ByteProcessor) imp.getProcessor();
                fp = bp.duplicate().convertToFloatProcessor();
                break;
            case ImagePlus.COLOR_RGB:
                ColorProcessor cp = (ColorProcessor) imp.getProcessor();
                fp = cp.duplicate().convertToFloatProcessor();
                break;
            case ImagePlus.GRAY16:
                ShortProcessor sp = (ShortProcessor) imp.getProcessor();
                fp = sp.duplicate().convertToFloatProcessor();
                break;
            case ImagePlus.GRAY32:
                fp = (FloatProcessor) imp.getProcessor();
                break;
            case ImagePlus.GRAY8:
                bp = (ByteProcessor) imp.getProcessor();
                fp = bp.duplicate().convertToFloatProcessor();
                break;
            default:
                break; 
        }

        Convolver convolver = new Convolver();
        float[] kernel = {1,-2,1,-2,4,-2,1,-2,1};
        convolver.convolve(fp, kernel, 3, 3);

        int w = fp.getWidth();
        int h = fp.getHeight();
        double sum = 0;

        for(int x = 1; x < (w-1); x++){
            for(int y = 1; y < (h-1); y++){
                sum += Math.abs(fp.getPixelValue(x, y));
            }
        }
        double sigma = Math.sqrt(Math.PI/2)*1.0/(6.0*(w-2)*(h-2))*sum;

        return sigma;
    }
    
    
    /**
     * Initialize needed Settings
     * @param sigma An estimate of the standard deviation of the Noise-Level 
     * within the Image
     * @param ip The Image-Processor of the original Image
     */
    private void initSettings(int sigma, ImageProcessor ip) {
        int type = new ImagePlus(null, ip).getType();
        
        // Init recommended Algorithm Settings
        double hfactor;
        if (type == ImagePlus.COLOR_256 || type == ImagePlus.COLOR_RGB) {
            
            // Color Image
            
            if (sigma > 0 && sigma <= 25) {
                n = 1;
                w = 10;
                hfactor = 0.55;
            } else if (sigma > 25 && sigma <= 55) {
                n = 2;
                w = 17;
                hfactor = 0.4;
            } else {
                n = 3;
                w = 17;
                hfactor = 0.35;
            }
        } else {
            
            // Gray Image
            
            if (sigma > 0 && sigma <= 15) {
                n = 1;
                w = 10;
                hfactor = 0.4;
            } else if (sigma > 15 && sigma <= 30) {
                n = 2;
                w = 10;
                hfactor = 0.4;
            } else if (sigma > 30 && sigma <= 45) {
                n = 3;
                w = 17;
                hfactor = 0.35;
            } else if (sigma > 45 && sigma <= 75) {
                n = 4;
                w = 17;
                hfactor = 0.35;
            } else {
                n = 5;
                w = 17;
                hfactor = 0.3;
            }
        }
        
        width = ip.getWidth();
        height = ip.getHeight();
        widthE = width + 2*w + 2*n;
        heightE = height + 2*w + 2*n;        

        // Read Pixels from ImageProcessor and store them in pixels Array
        convertPixels(ip, type);
        
        // Init Arrays
        uI = new int[dim][widthE*heightE];
        uD = new double[dim][widthE*heightE];
        wMaxArrI = new int[widthE*heightE];
        wMaxArrD = new double[widthE*heightE];
        wSumArrI = new int[widthE*heightE];
        wSumArrD = new double[widthE*heightE];
        
        double h = hfactor * sigma;
//        sigma2 = 2 * sigma * sigma * (dim * (2 * n + 1) * (2 * n + 1));
        sigma2 = 2 * sigma * sigma;
        distConst = (dim * (2 * n + 1) * (2 * n + 1));
        h2 = (h * h) / (dim * (2 * n + 1) * (2 * n + 1));
        
        // Multithreadding related Initializations
        nextdx = -w; 
        nextdy = -w;
    }
    
    /**
     * Returns next dx / dy Pair
     * dx and dy are needed to compute a specific iteration of the Algorithm.
     * This method provides the next unused dx / dy Pair to be used in a 
     * denoising Thread.
     * @return dx and dy as int array, in this respective order
     */
    private synchronized int[] getNextDV() {
        if (nextdy > 0) return null;
        
        int[] result = new int[] { nextdx, nextdy };

        if (nextdx == w) {
            nextdy++;
            nextdx = -w;
        } else {
            nextdx++;
        }
        
        return result;
    }
    
    /**
     * Converts the Image into its proper form sothat it can be used by the 
     * Algorithm
     * @param ip
     * @param type Type of the Image based on ImageJ ImageTypes
     */
    private void convertPixels(ImageProcessor ip, int type) {
        switch(type) {
            
            case ImagePlus.COLOR_256:
                convertColor256(ip);
                break;
            case ImagePlus.COLOR_RGB:
                convertRGB(ip);
                break;
            case ImagePlus.GRAY16:
                convertGray16(ip);
                break;
            case ImagePlus.GRAY32:
                convertGray32(ip);
                break;
            case ImagePlus.GRAY8:
                convertGray8(ip);
                break;
            default:
                break; 
        }
    }
    
    private void convertColor256(ImageProcessor ip) {
        dim = 1;
        
        byte[] pixelArray = (byte[]) ip.getPixels();
        pixels = new int[dim][width*height];
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                pixels[0][pos] = pixelArray[pos] & (0xff);
            }
        }
    }
    
    private void convertRGB(ImageProcessor ip) {
        dim = 3;
        
        int[] pixelArray = (int[])ip.getPixels();
        pixels = new int[dim][width*height];
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int qtemp = pixelArray[offset + x];
                pixels[0][offset + x] = ((qtemp & 0xff0000)>>16);
                pixels[1][offset + x] = ((qtemp & 0x00ff00)>>8);
                pixels[2][offset + x] = ((qtemp & 0x0000ff));
            }
        }
    }
    
    private void convertGray32(ImageProcessor ip) {
        dim = 1;
        
        float[] pixelArray = (float[]) ip.getPixels();
        pixels = new int[dim][width*height];
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                pixels[0][pos] = (int)pixelArray[pos];
            }
        }
    }
    
    private void convertGray16(ImageProcessor ip) {
        dim = 1;
        
        short[] pixelArray = (short[]) ip.getPixels();
        pixels = new int[dim][width*height];
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                pixels[0][pos] = (int)(pixelArray[pos] & (0xffff));
            }
        }
    }
    
    private void convertGray8(ImageProcessor ip) {
        dim = 1;
        
        byte[] pixelArray = (byte[]) ip.getPixels();
        pixels = new int[dim][width*height];
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                pixels[0][pos] = (int)(pixelArray[pos] & (0xff));
            }
        }
    }
    
    /**
     * Converts a denoised Picture back to its original Format and saves it
     * in the ImageProcessor
     * @param image
     * @param ip 
     */
    private void createPicture(double[][] image, ImageProcessor ip) {
        int type = imp.getType();
        
        switch(type) {
            
            case ImagePlus.COLOR_256:
                createPictureColor256(image, ip);
                break;
            case ImagePlus.COLOR_RGB:
                createPictureRGB(image, ip);
                break;
            case ImagePlus.GRAY16:
                createPictureGray16(image, ip);
                break;
            case ImagePlus.GRAY32:
                createPictureGray32(image, ip);
                break;
            case ImagePlus.GRAY8:
                createPictureGray8(image, ip);
                break;
            default:
                break; 
        }
        
//        imp.repaintWindow();

//        impNew.setTitle(newImageTitle);
//        impNew.show();
        
    }
    
    private void createPictureColor256(double[][] image, ImageProcessor ip) {
//        impNew = imp.createImagePlus();
//        impNew.setProcessor(imp.getProcessor().duplicate());
        byte[] pixelsPicture = (byte[])ip.getPixels();
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                byte pixel = (byte)(image[0][pos]);
                pixelsPicture[pos] = pixel;
            }
        }
        
        ip.setPixels(pixelsPicture);
    }
    
    private void createPictureRGB(double[][] image, ImageProcessor ip) {
//        impNew = imp.createImagePlus();
//        impNew.setProcessor(imp.getProcessor().duplicate());
        int[] pixelsPicture = (int[])ip.getPixels();
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int p = offset + x;
                int red = (int)image[0][p];
                int green = (int)image[1][p];
                int blue = (int)image[2][p];
                int pixel = ((red & 0xff)<<16) 
                          + ((green & 0xff)<<8) 
                          + (blue & 0xff);
                pixelsPicture[p] = pixel;
            }
        }
        ip.setPixels(pixelsPicture);
    }
    
    private void createPictureGray32(double[][] image, ImageProcessor ip) {
//        impNew = imp.createImagePlus();
//        impNew.setProcessor(imp.getProcessor().duplicate());
        float[] pixelsPicture = (float[])ip.getPixels();
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                float pixel = (float)(image[0][pos]);
                pixelsPicture[pos] = pixel;
            }
        }
        
        ip.setPixels(pixelsPicture);
    }
    
    private void createPictureGray16(double[][] image, ImageProcessor ip) {
//        impNew = imp.createImagePlus();
//        impNew.setProcessor(imp.getProcessor().duplicate());
        short[] pixelsPicture = (short[])ip.getPixels();
        
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                short pixel = (short)(image[0][pos]);
                pixelsPicture[pos] = pixel;
            }
        }
        
        ip.setPixels(pixelsPicture);
    }
    
    private void createPictureGray8(double[][] image, ImageProcessor ip) {
//        ImagePlus impNew = imp.createImagePlus();
//        impNew.setProcessor(imp.getProcessor().duplicate());
        byte[] pixelsPicture = (byte[])ip.getPixels();
        for (int y = 0; y < height; y++) {
            int offset = y*width;
            for (int x = 0; x < width; x++) {
                int pos = offset + x;
                byte pixel = (byte)(image[0][pos]);
                pixelsPicture[pos] = pixel;
            }
        }
        
        ip.setPixels(pixelsPicture);
    }
    
    public static BufferedImage convertToImage(int width, int height, int[] pixels) {
        int wh = width * height;
        int[] newPixels = new int[wh*3];
        for (int i = 0; i < wh; i++) {
            int rgb = pixels[i];
            int red = (rgb >> 16) & 0xFF;
            int green = (rgb >> 8) & 0xFF;
            int blue = rgb & 0xFF;
            newPixels[i*3] = red;
            newPixels[i*3 + 1] = green;
            newPixels[i*3 + 2] = blue;
        }
        
        BufferedImage image = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
        WritableRaster raster = (WritableRaster) image.getData();
        raster.setPixels(0,0,width,height,newPixels);
        image.setData(raster);
        
        return image;
    }
    
    /*
     * Worker Thread for the Non-Local Means Algorithm. Uses its own Arrays
     * to store its result. If getNextDV() returns an empty array, the results
     * get delivered and the Thread ends.
     */
    class WorkerInt extends Thread {
        private int[] TwMaxArr;
        private int[] TwSumArr;
        private int[][] TimagePart;
        private int[][] TS;
        

        @Override
        public void run() {
            int[] vec;
            int dx, dy;
            TwMaxArr = new int[widthE*heightE]; // Weight Max Array
            TwSumArr = new int[widthE*heightE]; // Weight Sum Array
            TimagePart = new int[dim][widthE*heightE]; // Partly Denoised Image
            TS = new int[dim][widthE*heightE]; // Difference Image
            
            vec = getNextDV(); // get first dx / dy pair
            while (vec != null) {
                dx = vec[0];
                dy = vec[1];
                if ((2*w + 1) * dx + dy >= 0) {
                    // redundant, get next pair
                    vec = getNextDV();
                    continue;
                }
                
                // compute Sdx
                computeDifferenceImageInt(pixelsExpand, TS, dx, dy);

                // denoise with Sdx
                denoiseInt(TimagePart, TS, TwMaxArr, TwSumArr, dx, dy);
                
                // get next Vector
                vec = getNextDV();
            }
            
            // save to global variables
            deliverImagePartInt(TimagePart);
            deliverWMaxArrInt(TwMaxArr);
            deliverWSumArrInt(TwSumArr);
        }
    }


    class WorkerDouble extends Thread {
        private double[] TwMaxArr;
        private double[] TwSumArr;
        private double[][] TimagePart;
        private double[][] TS;
        

        @Override
        public void run() {
            int[] vec;
            int dx, dy;
            TwMaxArr = new double[widthE*heightE];
            TwSumArr = new double[widthE*heightE];
            TimagePart = new double[dim][widthE*heightE];
            TS = new double[dim][widthE*heightE];
            
            vec = getNextDV();
            while (vec != null) {
                dx = vec[0];
                dy = vec[1];
                if ((2*w + 1) * dy + dx >= 0) {
                    vec = getNextDV();
                    continue;
                }
                
                // compute Sdx
                computeDifferenceImageDouble(pixelsExpand, TS, dx, dy);

                // denoise with Sdx
                denoiseDouble(TimagePart, TS, TwMaxArr, TwSumArr, dx, dy);
                
                // get next Vector
                vec = getNextDV();
            }
            
            // save to global variables
            deliverImagePartDouble(TimagePart);
            deliverWMaxArrDouble(TwMaxArr);
            deliverWSumArrDouble(TwSumArr);
        }
    }
}